#ifndef MATCHPROJECTIONPOINT_H 
#define MATCHPROJECTIONPOINT_H

#include <ros/ros.h>
#include <vector>
#include <utility>
#include <cmath>
#include <limits>
#include <iostream>
#include <algorithm>
#include <glog/logging.h>
#include "cached_path_profile.h"
#include "paths_struct.h"


class Match_Projection_Point
{
public:

    Match_Projection_Point();
    ~Match_Projection_Point();
    std::pair<double, double> VectorFromPointToPoint(const std::pair<double, double>& pointA, const std::pair<double, double>& pointB);
    double DistanceBetweenPoints(const std::pair<double, double>& pointA, const std::pair<double, double>& pointB);
    std::pair<double, double> CalculateProjectionPoint(const std::pair<double, double>& current_odom, 
                                                                                                                const std::pair<double, double>& closest_point, 
                                                                                                                const std::vector<std::pair<double, double>>& points);
    PointAndIndex match_and_projection_point(const std::pair<double, double>& odom_receive_, 
                                                                                                                        const std::vector<std::pair<double, double>>& points);
    bool ComputePathProfileWithCache(const std::vector<std::pair<double, double>> &xy_points, CachedPathProfile &cache);

private:

    int last_closest_index = -1;
    std::pair<double, double> projection_point;
};

#endif // MATCHPROJECTIONPOINT_H